#include <iostream>
#include <stdio.h>
#include <cmath>
#include <string>
#include <vector>

#include <ros/ros.h> 
#include <sensor_msgs/NavSatFix.h>

using namespace std;

#define BASE_TIME 1609307878

struct gps_data{
    double sec;
    double lat;
    double lon;
    double alt;
    double pos_accuracy;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "gps_node");
    ros::NodeHandle n("~");

    ros::Publisher pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000);

    ros::Duration(1).sleep();

    printf("read dataPath: %s\n", argv[1]);
    string dataPath = argv[1];

    FILE* file;
    file = std::fopen((dataPath + "/gps.csv").c_str(), "r");
    if(file == NULL){
	    printf("cannot find file: %s \n", dataPath.c_str());
	    ROS_BREAK();
	    return 0;    
    }
    char string_Temp[60];
    ROS_INFO("Reading head ");
    for(int i=0;i<10;i++)
    {
        fscanf(file, "%s",&string_Temp);
    }
    ROS_INFO("Reading data ");
    vector<gps_data> gpsList;
    long nanosec;
    double lat, lon, alt, accuracy, declination, BD_lat, BD_lon, BD_alt, BD_acc;
    while(fscanf(file, "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", &nanosec, &lon, &lat, &alt, &accuracy, &declination, &BD_lat, &BD_lon, &BD_alt, &BD_acc) != EOF)
    {
        double sec = nanosec/1000000000.0;
        gps_data data;
        data.sec = sec;
        data.alt = alt;
        data.lat = lat;
        data.lon = lon;
        data.pos_accuracy = accuracy;
        gpsList.push_back(data);
    }
    std::fclose(file);
    clock_t start,end;
    for(int i = 0; i < gpsList.size(); i++)
    {
        start = clock(); 
        if(ros::ok())
        {
            printf("\nprocess gps %d\n", (int)i);
            double time = BASE_TIME + gpsList[i].sec;
            sensor_msgs::NavSatFix gps_position;
            gps_position.header.frame_id = "NED";
			gps_position.header.stamp = ros::Time(time);
            gps_position.latitude  = gpsList[i].lat;
			gps_position.longitude = gpsList[i].lon;
			gps_position.altitude  = gpsList[i].alt;
			gps_position.position_covariance[0] = gpsList[i].pos_accuracy;

            pubGPS.publish(gps_position);
             
        }
        else
            break;
        end = clock();
        double during =  double(end-start)/CLOCKS_PER_SEC;
        if(i < gpsList.size()-1)
        {
            double duration = gpsList[i+1].sec - gpsList[i].sec - during;
            ros::Duration(duration).sleep();
        }
    }
    return 0;
}